#pragma config(Sensor, in1,    midSen,         sensorLineFollower)
#pragma config(Sensor, in2,    leftSen,        sensorLineFollower)
#pragma config(Sensor, in3,    rightSen,       sensorLineFollower)
#pragma config(Sensor, in4,    lightSen,       sensorReflection)
#pragma config(Sensor, dgtl1,  touchStart,     sensorTouch)
#pragma config(Sensor, dgtl2,  sonarSen,       sensorSONAR_inch)
#pragma config(Motor,  port2,           leftMot,       tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           rightMot,      tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define Dodge1 1
#define Dodge2 2
#define stopped 3
#define rightstate 4
#define leftstate 5
#define strightstate 6
#define betweenObstacles 7
#define beforeObstacle1 8
#define afterObstacle2 9

bool goingLeft() {
  int leftSense = SensorValue(leftSen);
  if(leftSense < 1600)
  {
    return true;
  }

  else {
    return false;
  }
}

bool goingStright(){
  int midSense = SensorValue(midSen);
  if(midSense < 1600)
  {
    return true;
  }

  else {
    return false;
  }
}

bool goingRight() {
  int rightSence = SensorValue(rightSen);
  if(rightSence < 1600)
  {
    return true;
  }
  else {
    return false;
  }
}
bool obstacle1InSight(){
  int sonarSense = SensorValue(sonarSen);
  if(sonarSense < 8  && sonarSense !=-1) {
    return true;
  }
  else
  {
    return false;
  }

}

task main()
{
  int state;
  state=stopped;

  while(true)
  {

    switch(state)
    {
    case stopped:
      int touchSense = SensorValue(touchStart);
      if(touchSense == 1)
      {

        state = strightstate;
      }
      break;

    case strightstate:
    motor[leftMot]=40;
        motor[rightMot]=40;
      if(obstacle1InSight())
      {

        motor[leftMot] = -60;
        motor[rightMot] = 60;
        wait1Msec(1000);
     state = Dodge1;

}


      else if (goingLeft())
      {
        state = rightstate;
        motor[leftMot]=-40;
        motor[rightMot]=60;
      }
      else if (goingRight())
      {
        state = leftstate;
        motor[leftMot]=60;
        motor[rightMot]=-40;
      }
      break;

    case rightstate:
      if (goingStright())
      {
        state =  strightstate;
        motor[leftMot]=40;
        motor[rightMot]=40;
      }
      break;

    case leftstate:
      if (goingStright())
      {
        motor[leftMot]= 40;
        motor[rightMot]= 40;
        state =  strightstate;
      }
      break;

    case Dodge1:
          motor[leftMot] = 120;
        motor[rightMot] = 10;
        int midSense = SensorValue(midSen);
         int rightSence = SensorValue(rightSen);
           int leftSense = SensorValue(leftSen);
      if(leftSense < 1600){
  motor[leftMot] = -40;
        motor[rightMot] = 60;
        state=strightstate;}

 else if(rightSence < 1600)
   {
  motor[leftMot] = -40;
        motor[rightMot] = 60;
        state=strightstate;
        }

 else if(midSense < 1600){
  motor[leftMot] = -40;
        motor[rightMot] = 60;
        state=strightstate;
      }
      break;

     }

  }
  }
